\chapter{Oprogramowanie sterujące}

Na podstawie koncepcji systemu sterującego rozgrywką w lidze \w{Small Size}, przedstawionej w rozdziale~\ref{sec:opisAlgorytmu}, zostało stworzone oprogramowanie pozwalające na symulację gry z użyciem \w{Gazebo}, aplikacji kontrolującej robota oraz szeregu programów pomocniczych. Opracowane rozwiązanie miało za zadanie wiernie odzwierciedlać elementy systemu ligi po to, aby przystosowanie części oprogramowania sterującego do kontrolowania rzeczywistych robotów było możliwie najmniej czasochłonne. W wyniku prac powstało pięć modułów napisanych w języku C++, zawierających 100 plików źródłowych liczących ponad 7500 linii kodu. Szczegóły architektury stworzonego oprogramowania oraz opis poszczególnych komponentów zamieszczono poniżej.

\section{Architektura}
Komponenty opracowanego środowiska testowego przedstawiono na rysunku~\ref{fig:architektura_final}. Rozgrywka odbywa się w symulatorze \w{Gazebo}, z którym za pomocą metod dostarczanych przez bibliotekę \w{libgazebo} komunikują się aplikacje klienckie. Odpowiednikiem ligowego oprogramowania \w{SSL-Vision}, które przetwarza obraz z kamery, jest program \texttt{videoserver} - łączy się on z interfejsem \texttt{Simulation}, który pozwala na pobranie danych o obiektach znajdujących się na boisku. Informacja ta jest następnie rozgłaszana (\w{multicast}) poprzez sieć IP do pozostałych aplikacji.
        \begin{figure}[!t]
        \centering
        \includegraphics[width=0.95\textwidth]{./architektura_final}
        \caption{Komponenty środowiska testowego ligi \w{Small Size}\label{fig:architektura_final}}
	\end{figure}

Kolejnym programem wchodzącym w skład opracowanego środowiska testowego jest \texttt{auto\_referee}. Wysyła on do aplikacji sterujących robotami komunikaty o~rozpoczęciu i zakończeniu gry. Ponadto na podstawie danych otrzymywanych od serwera video, informuje on o strzelonych golach, autach czy rzutach rożnych. Program ma także możliwość automatycznego ustawiania piłki (za pomocą interfejsu \texttt{Simulation}) w miejscu odpowiednim dla stałego fragmentu gry. Dzięki temu rozgrywka nie wymaga ingerencji człowieka\footnote{Podczas meczów \w{Small Size} aplikacja sędziowska służy jedynie do wysyłania komunikatów do robotów. Ustawianie piłki w odpowiednich miejscach to obowiązek sędziów.}, co umożliwia automatyzację eksperymentów związanych z działaniem algorytmów sterujących grą. 

Informacje wysyłane przez \texttt{videoserver} oraz \texttt{auto\_referee} są wykorzystywane przez aplikację \texttt{autonomic\_client} sterującą robotem. Na rysunku~\ref{fig:architektura_final} przedstawiono tylko dwie z nich, kontrolujące modele \w{yellow0} oraz \w{blue0}. W rzeczywistości w~systemie musi działać tyle aplikacji, ile robotów bierze udział w rozgrywce (2 drużyny po 5 zawodników) - wynika to z faktu, że aplikacja sprawuje kontrolę tylko nad jednym modelem (sterowanie rozproszone). Rolą programu \texttt{autonomic\_client} jest wyznaczanie instrukcji sterujących, które są przekazywane odpowiednim kontrolerom modelów w \w{Gazebo} za pomocą interfejsów \texttt{Position}, dostarczanych przez symulator.

Oprócz wspomnianych modułów w skład oprogramowania wchodzi biblioteka \texttt{mgr\_common}, dostarczająca zbioru klas współdzielonych przez aplikacje, oraz program \texttt{mgr\_tests}, zarządzający przeprowadzaniem eksperymentów w symulowanym środowisku.

\section{Opis komponentów oprogramowania \label{sec:komponenty_oprogramowania}}

    \lstset{language=C++,  basicstyle=\ttfamily\footnotesize, 
    commentstyle={}, xleftmargin=20pt, numbers=left, numberstyle=\footnotesize,
    caption={},
    backgroundcolor=\color{light-gray},
    frame=single,
    breaklines=true}

\subsection{mgr\_common}
Moduł ten stanowi statyczną bibliotekę klas i funkcji współdzielonych przez aplikacje. Trzy najważniejsze komponenty przez nią dostarczane to:

\begin{itemize}
 \item \texttt{VideoClient} - klasa klienta UDP, dostarczająca funkcjonalność polegającą na odbieraniu komunikatów przesyłanych przez moduł serwera video; używana przez aplikacje \autonomicClient{} oraz \autoReferee{}; odebrane dane są przetrzymywane w klasie \texttt{Videodata} o następujących polach: 
\begin{lstlisting}[name=videodata]
class Videodata {
public:
	double x; 	///< x position	
	double y; 	///< y position	
	double rot; 	///< rotation in radians	
	double vx; 	///< x velocity	
	double vy; 	///< y velocity
}
\end{lstlisting}

 \item \texttt{RefBoxClient} - analogicznie do klasy \texttt{VideoClient}, odpowiada za odbieranie danych przesyłanych przez  sędziego; wykorzystywana przez aplikacje \autonomicClient{} oraz \mgrTests; protokół komunikacji implementowany przez tę klasę jest w pełni zgodny z rozwiązaniem stosowanym przez ligową aplikację \w{Referee Box},
 \item \texttt{utils} - zestaw przydatnych klas narzędziowych, takich jak \texttt{Line}, \texttt{Rectangle}, \texttt{Angle},  \texttt{Vector2d} czy też \texttt{Position2d}; używane przez wszystkie moduły do przeprowadzania obliczeń geometrycznych, opisywania prędkości lub pozycji modeli na planszy itd.
\end{itemize}


\subsection{videoserver}

Zadanie aplikacji \videoserver{} polega na rozsyłaniu za pomocą sieci IP informacji o współrzędnych oraz prędkościach modeli biorących udział w symulacji. Komunikacja z symulatorem odbywa się za pomocą obiektu klasy \texttt{SimulationIface}. Aplikacja odpytuje \w{Gazebo} o parametry modeli, a następnie rozsyła je w formie komunikatów za pomocą protokołu UDP. 
Podczas uruchomienia programu należy określić położenie pliku konfiguracyjnego z listą modeli, które mają być monitorowane. Możliwe jest także konfigurowanie używanego przez aplikację adresu \w{multicast} oraz numeru portu.

\subsection{auto\_referee}

Architekturę aplikacji automatycznego sędziego przedstawiono na rysunku~\ref{fig:architektura_autoReferee}. Program składa się z czterech głównych komponentów:
\begin{itemize}
 \item klasy \texttt{Analyzer}, odpowiadającej za ocenę sytuacji na boisku oraz wydawanie decyzji sędziowskich,
 \item klasy \texttt{Server}, transmitującej informacje o podjętej decyzji do sieci IP za pomocą protokołu UDP,
 \item klas \texttt{VideoClient} oraz \texttt{GazeboClient}, odpowiadających za odbieranie danych z \texttt{videoservera} oraz komunikację z symulatorem.
\end{itemize}

        \begin{figure}[!b]
        \centering
        \includegraphics[width=0.6\textwidth]{./architektura_autoReferee}
        \caption{Automatyczny sędzia - komponenty aplikacji\label{fig:architektura_autoReferee}}
	\end{figure}

Za każdym razem, gdy otrzymane zostaną nowe dane z serwera video, blok analizujący wykonuje szereg geometrycznych sprawdzeń, próbując wykryć sytuacje wymagające wydania decyzji sędziego (zdobycie bramki, aut, rzut wolny itp.). Werdykt transmitowany jest do aplikacji sterujących robotami za pomocą protokołu zgodnego ze używanym w lidze \w{Small Size}, który jest stosowany w oficjalnej aplikacji sędziowskiej \w{Referee Box}\footnote{\url{http://small-size.informatik.uni-bremen.de/referee:start}}. Dzięki temu zapewniona jest pełna zgodność z rozwiązaniem ligowym. Dodatkowo automatyczny sędzia jest w stanie zapewnić odpowiednie ustawienie piłki po zarządzeniu wykonania stałego fragmentu gry - służy do tego moduł komunikujący się z \w{Gazebo}.

\begin{table}[!t]
 \centering
 \begin{tabular}{|l|c|c|}
  \hline
  \multirow{2}{*}{ \textbf{Nazwa komendy}} &    \multicolumn{2}{c|}{ \textbf{Kod}}	\\
  \cline{2-3}	 & \textbf{drużyna żółta} & \textbf{drużyna niebieska}\\
  \hline
  rzut wolny	& \hspace{1.55cm} k	\hspace{1.55cm}	& \hspace{1.5cm} K \hspace{1.5cm}				\\
  \hline
  rzut karny 	& p		& P			\\
  \hline
  gol		& g		& G			\\  
  \hline
  zatrzymaj 	& \multicolumn{2}{c|}{H}		\\
  \hline
  stop		& \multicolumn{2}{c|}{S}		\\
  \hline
  gotowy	& \multicolumn{2}{c|}{' '}	\\
  \hline
  start		& \multicolumn{2}{c|}{s}		\\  
  \hline
 \end{tabular}
   \caption{Kody ważniejszych komunikatów sędziego\label{tab:komendy}}
\end{table}

Struktura przesyłana do aplikacji sterujących robotami wygląda następująco:
\begin{lstlisting}[name=pakiet_autoreferee]
struct GameStatePacket {    
    char cmd;
    unsigned char cmd_counter; 	   
    unsigned char goals_blue; 	   
    unsigned char goals_yellow;
    unsigned short time_remaining;
};
\end{lstlisting}
Pole \texttt{cmd} zawiera aktualną komendę sędziego (listę ważniejszych komend zawiera tabela~\ref{tab:komendy}). Pozostałe parametry informują o ilości wysłanych komend, liczbie bramek zdobytych przez drużyny oraz czasie pozostałym do zakończenia gry. Powyższy pakiet jest retransmitowany co pewien ustalony czas (w aplikacji przyjęto $T=1~s$). 


\subsection{autonomic\_client}

        \begin{figure}[!t]
        \centering
        \includegraphics[width=0.9\textwidth]{./architektura_robot}
        \caption{Główne klasy aplikacji \autonomicClient\label{fig:architektura_robot}}
	\end{figure}

Zadaniem tej aplikacji jest sterowanie konkretnym modelem robota w symulowanym środowisku. Odpowiada ona zarówno za  bezkolizyjne poruszanie się robota po boisku, jak i za generowanie strategii dla zawodnika. 

        \begin{wrapfigure}{R}{0.45\textwidth}
        \centering
        \includegraphics[width=0.4\textwidth]{./robot_class}
        \caption{Główna klasa aplikacji \autonomicClient\label{fig:robot_class}}
	\end{wrapfigure}

Elementy składowe architektury programu przedstawiono na rysunku~\ref{fig:architektura_robot}. Klasą nadrzędną, agregującą pozostałe elementy aplikacji, jest klasa \texttt{Robot}. Wykonywanie programu sprowadza się do wywoływania w~pętli głównej metody wspomnianej klasy - \texttt{void mainLoop()}. Powoduje ona aktualizację obiektu stanu gry - \texttt{GameState}. Klasa ta realizuje wzorzec projektowy obserwatora - w momencie, kiedy aplikacja otrzyma nowe dane z serwera video lub od sędziego, obiekt \texttt{GameState} powiadamia zarejestrowanych słuchaczy o zmianie stanu gry. W reakcji na taką zmianę obiekt klasy \texttt{Robot} aktualizuje blok \texttt{AI}, odpowiadający za strategię zawodnika.

Zgodnie z rozwiązaniem zaproponowanym w rozdziale~\ref{sec:generowanie_strategii}, logika związana z procesem generowania strategii została podzielona pomiędzy trzy klasy, przedstawione na rysunku~\ref{fig:ai_class}. Implementację algorytmu sterowania bramkarzem realizuje metoda \texttt{execute()} klasy \texttt{GoalkeeperAI}. Pozostałe klasy przedstawione na diagramie odpowiadają za kontrolowanie zawodnika grającego w polu - w sytuacji zatrzymania gry oraz podczas toczącej się rozgrywki.


        \begin{figure}[!t]
        \centering
        \includegraphics[width=0.6\textwidth]{./ai_class}
        \caption{Warianty klas generujących strategię robota\label{fig:ai_class}}
	\end{figure}

        \begin{figure}[!t]
        \centering
        \includegraphics[width=0.95\textwidth]{./tasks_class}
        \caption{Hierarchia klas realizujących zadania\label{fig:tasks_class}}
	\end{figure}

        \begin{wrapfigure}{R}{0.6\textwidth}
        \centering
        \includegraphics[width=0.55\textwidth]{./driver_class}
        \caption{Sterownik robota\label{fig:driver_class}}
	\vspace{-10pt}
	\end{wrapfigure}

Zadania wykonywane przez algorytm sterowania robotem zostały zrealizowane zgodnie z koncepcją przedstawioną w !sekcji~\ref{sec:zadania}. Diagram klas pokazujący sposób implementacji przedstawiono na rysunku~\ref{fig:tasks_class}.
W~przypadku, gdy w trakcie wykonywania zadań niezbędne jest wyznaczenie ścieżki ruchu do konkretnego punktu na boisku, obiekty wspomnianych klas korzystają z implementacji opisanych w sekcji~\ref{sec:planowanie_ruchu} algorytmów RRT oraz metody sztucznych pól potencjału, dostarczanych przez klasy \texttt{RRT} oraz \texttt{PotentialField}. Ostatnia z wymienionych metod jest stosowana tylko w przypadku realizacji zadania \texttt{Stop} - sprawdza się ona idealnie w sytuacji, kiedy należy utrzymać roboty w regulaminowej odległości $50~cm$ od piłki ustawianej do wykonania stałego fragmentu gry.

Najniższą warstwą oprogramowania robota jest jego sterownik, implementowany przez obiekt klasy \texttt{Driver}. Komunikuje się on bezpośrednio z symulatorem \w{Gazebo} za pomocą interfejsu \w{Position} odpowiadającego sterowanemu modelowi. Diagram klasy przedstawiono na rysunku~\ref{fig:driver_class}. Jak widać, dostarcza ona szeregu metod, pozwalających na manipulację modelem: ustawianie prędkości liniowej oraz kątowej, kierowanie robota w stronę celu, sprawdzanie stanu \w{kickera} oraz kopanie (\texttt{kick()}) lub podawanie piłki (\texttt{pass()} - używająca mniejszej siły). Metody związane z regulacją położenia robota (jak np. \texttt{setRotation()}) korzystają z implementacji regulatora PID z odpowiednio dobranymi parametrami. Ponadto należy zwrócić uwagę na sposób realizacji obracania modelu. Dostępne są dwa warianty - prosty obrót wokół punktu centralnego robota (metoda \texttt{rotateTowards()}) oraz wariant stosowany w momencie, kiedy zawodnik jest w posiadaniu piłki - metoda \texttt{rotateTowardsWithBall()}. W tym przypadku robot cały czas jest zwrócony w stronę piłki i oś obrotu znajduje się w jej centralnym punkcie. W takich warunkach ważne jest, aby model poruszał się po łuku $c = \frac{w}{v}$, gdzie $c = \frac{1}{r}$ jest równe odwrotności odległości pomiędzy robotem a piłką, natomiast prędkość liniowa robota $v = [v_x,0]$ jest określana przez zadanie jedynie składowej prostopadłej do odcinka łączącego model i piłkę. Zakładając, że wartości $r$ oraz $v$ są znane, prędkość kątowa robota wynosi $w = \frac{v}{r}$, zaś o kierunku obrotu decyduje zwrot składowej $v_x$ prędkości liniowej. Ilustrację tego rozwiązania przedstawiono na rysunku~\ref{fig:obrot_pilka}.

       \begin{figure}[!b]
	\vspace{0.5cm}
        \centering
        \includegraphics[width=0.86\textwidth]{./obrot_pilka}
	\vspace{0.5cm}
        \caption{Zasada wykonywania obrotu z piłką\label{fig:obrot_pilka}}
	\end{figure}

Działanie programu \autonomicClient{} można konfigurować za pomocą parametrów podawanych podczas uruchamiania aplikacji. Dzięki temu możliwa jest realizacja modyfikacji strategii zaproponowanych sekcji~\ref{sec:modyfikacje}. Parametry pozwalają określić rolę, jaką będzie posługiwał się zawodnik na boisku, a także włączyć funkcje równomiernego rozmieszczania robotów oraz krycia przeciwnika.

\subsection{mgr\_tests}
Ostatnim z komponentów opracowanego środowiska jest aplikacja \texttt{mgr\_tests}, odpowiadająca za wykonywanie eksperymentów, które opisano w rozdziale~\ref{sec:testy}. Korzysta ona z zestawu skryptów powłoki \w{bash}, które pozwalają na uruchomienie komponentów środowiska testowego (symulatora \w{Gazebo}, serwera video, sędziego oraz aplikacji sterujących wszystkimi robotami) z odpowiednimi parametrami. Skrypty definiują głównie opcje związane z programami \autonomicClient{}, a więc to, którym modelem steruje dany program i jakie są parametry związane z generacją strategii dla kontrolowanego modelu.

Aplikacja jest w stanie uruchamiać konfigurowalną liczbę eksperymentów o zadanym czasie trwania. Jej rolą jest także zliczanie bramek, co oznacza, że program musi odbierać dane od sędziego (wykorzystywany jest ponownie obiekt klasy \texttt{RefBoxClient} z biblioteki \texttt{mgr\_common}). Po skończeniu eksperymentu \texttt{mgr\_tests} zapisuje wyniki meczów oraz inne statystyki do plików tekstowych, które mogą podlegać dalszej analizie.

